#include <moying_costmap/moying_costmap.h>
#include <SDL/SDL_image.h>
#include <fstream>
#include <cstring>
#include <stdexcept>
#include <stdlib.h>
#include <stdio.h>
#include <algorithm> 

namespace moying
{
    namespace navigation
    {
        CostMap::CostMap(): is_ready_(false)
        {
        }

        CostMap::~CostMap()
        {
            
        }

        void CostMap::getRaw1DCosts(std::vector<unsigned char> &_1dcost)
        {
            _1dcost.resize(raw_size_x_ * raw_size_y_);
            for(int x = 0; x < raw_size_x_; x++)
            {
                for(int y = 0; y < raw_size_y_; y++)
                {
                   _1dcost[y * raw_size_x_ + x] = raw_costs_[x][y];
                }
            }
        }

        void CostMap::get1DCosts(std::vector<unsigned char> &_1dcost)
        {
            _1dcost.resize(size_x_ * size_y_);
            for(int x = 0; x < size_x_; x++)
            {
                for(int y = 0; y < size_y_; y++)
                {
                   _1dcost[y * size_x_ + x] = costs_[x][y];
                }
            }
        }


        void CostMap::setObstacle(int mx, int my)
        {
            costs_[mx][my] = MAPCOSTS::OBSTACLE;
        }

        bool CostMap::rawMapToWorld(int mx, int my, double& wx, double& wy) 
        {
            wx = origin_x_ + (mx + 0.5) * raw_resolution_;
            wy = origin_y_ + (my + 0.5) * raw_resolution_;
            return true;
            
            // wx = origin_x_ + mx * resolution_;
            // wy = origin_y_ + my * resolution_;
        }

        bool CostMap::mapToWorld(int mx, int my, double& wx, double& wy) 
        {
            int rmx,rmy;
            if( !mapToRawMap(mx,my,rmx,rmy) )
                return false;
            return rawMapToWorld(rmx,rmy,wx,wy);

            // wx = origin_x_ + (mx + 0.5) * resolution_;
            // wy = origin_y_ + (my + 0.5) * resolution_;
            // wx = origin_x_ + mx * resolution_;
            // wy = origin_y_ + my * resolution_;
            // return true;
        }

        bool CostMap::worldToRawMap(double wx, double wy, int& mx, int& my) 
        {
            if (wx < origin_x_ || wy < origin_y_)
                return false;
            mx = (int)((wx - origin_x_) / raw_resolution_);
            my = (int)((wy - origin_y_) / raw_resolution_);
            return mx < raw_size_x_ && my < raw_size_y_;
        }
        
        bool CostMap::worldToMap(double wx, double wy, int& mx, int& my) 
        {
            int rmx,rmy;
            if( !worldToRawMap(wx,wy,rmx,rmy))
                return false;
            return rawMapToMap(rmx,rmy,mx,my);

            // if (wx < origin_x_ || wy < origin_y_)
            //     return false;
            // mx = (int)((wx - origin_x_) / resolution_);
            // my = (int)((wy - origin_y_) / resolution_);
            // return mx < size_x_ && my < size_y_;
        }

        unsigned char CostMap::getObstacleCost()
        {
            return MAPCOSTS::OBSTACLE;
        }

        std::vector<std::vector<unsigned char>>* CostMap::get_cost_map()
        {
            return &costs_;
        }

        std::vector<std::vector<unsigned char>>* CostMap::get_raw_cost_map()
        {
            return &raw_costs_;
        }

        void CostMap::normalize_map()
        {
            ;
        }

        bool CostMap::loadStaticMapFromFile(const std::string &fname, double res, bool negate, double occ_th, double free_th, std::vector<double> &origin)
        {
            std::ifstream fin(fname.c_str());
            if (fin.fail()) {
                std::cout<<"Map_server could not open "<<fname<<std::endl;
                // exit(-1);
                return false;
            }else{
                INFO("open map file : "<<fname);
            }

            SDL_Surface* img;

            unsigned char* pixels;
            unsigned char* p;
            unsigned char value;
            int rowstride, n_channels, avg_channels;
            int i,j;
            int k;
            double occ;
            int alpha;
            int color_sum;
            double color_avg;

            // Load the image using SDL.  If we get NULL back, the image load failed.
            if(!(img = IMG_Load(fname.c_str())))
            {
                std::string errmsg = std::string("failed to open image file \"") +
                        fname + std::string("\": ") + IMG_GetError();
                // throw std::runtime_error(errmsg);
                ERROR(errmsg);
                return false;
            }

            HIGHLIGHT("Loading map");
            // Copy the image data into the map structure
            raw_size_x_ = img->w;
            raw_size_y_ = img->h;
            raw_resolution_ = res;
            origin_x_ = origin[0];
            origin_y_ = origin[1];
            origin_th_ = origin[2];
            // btQuaternion q;
            // q.setEulerZYX(*(origin+2), 0, 0);
            // resp->map.info.origin.orientation.x = q.x();
            // resp->map.info.origin.orientation.y = q.y();
            // resp->map.info.origin.orientation.z = q.z();
            // resp->map.info.origin.orientation.w = q.w();

            raw_costs_.resize(raw_size_x_);
            for(int i = 0; i < raw_size_x_; i++)
                raw_costs_[i].resize(raw_size_y_);

            // Get values that we'll need to iterate through the pixels
            rowstride = img->pitch;
            n_channels = img->format->BytesPerPixel;

            // NOTE: Trinary mode still overrides here to preserve existing behavior.
            // Alpha will be averaged in with color channels when using trinary mode.
            if (!img->format->Amask)
                avg_channels = n_channels;
            else
                avg_channels = n_channels - 1;

            // Copy pixel data into the map structure
            pixels = (unsigned char*)(img->pixels);
            for(j = 0; j < raw_size_y_; j++)
            {
                for (i = 0; i < raw_size_x_; i++)
                {
                // Compute mean of RGB for this pixel
                p = pixels + j*rowstride + i*n_channels;
                color_sum = 0;
                for(k=0;k<avg_channels;k++)
                    color_sum += *(p + (k));
                color_avg = color_sum / (double)avg_channels;

                if (n_channels == 1)
                    alpha = 1;
                else
                    alpha = *(p+n_channels-1);

                // if(negate)
                //     color_avg = 255 - color_avg;


                // If negate is true, we consider blacker pixels free, and whiter
                // pixels occupied.  Otherwise, it's vice versa.
                occ = (255 - color_avg) / 255.0;

                // Apply thresholds to RGB means to determine occupancy values for
                // map.  Note that we invert the graphics-ordering of the pixels to
                // produce a map with cell (0,0) in the lower-left corner.
                // if(occ > occ_th)
                //     value = +100;
                // else if(occ < free_th)
                //     value = 0;
                // else if(alpha < 1.0)
                //     value = -1;
                // else {
                //     double ratio = (occ - free_th) / (occ_th - free_th);
                //     value = 99 * ratio;
                // }

                // if(occ >= occ_th)
                //     value = MAPCOSTS::OBSTACLE;
                // else 
                //     value = MAPCOSTS::FREE;

                value = occ < free_th ? MAPCOSTS::FREE : MAPCOSTS::OBSTACLE;
                raw_costs_[i][raw_size_y_ - j - 1] = value;
                // raw_costs_[i][j] = value;

                // if(i == 206 && raw_size_y_ - j - 1 == 137){
                //     printf("!!!!! value=%d color:%.2f free_th=%.2f\n",(int)value,color_avg,free_th);
                // }
                }
            }


            SDL_FreeSurface(img);
            HIGHLIGHT("Original map loaded");
                
            return true;
        }

        void CostMap::inflateStaticMap(double inflation_radius)
        {
            int inflation_num = round(inflation_radius/raw_resolution_);
            HIGHLIGHT("Inflatge num "<<inflation_num);
            int i,j,k,l;
            for(i = 0;i < raw_size_x_; i++)
            {
                for(j = 0;j < raw_size_y_; j++)
                {
                    int x_edge_down = std::max( 0 , i - inflation_num);
                    int x_edge_up = std::min(raw_size_x_-1, i + inflation_num);
                    int y_edge_down = std::max(0,j - inflation_num);
                    int y_edge_up = std::min(raw_size_y_-1,j + inflation_num);

                    if(raw_costs_[i][j] == MAPCOSTS::OBSTACLE)
                    {
                        for(k = x_edge_down; k <= x_edge_up; k++)
                        {
                            for(l = y_edge_down; l <= y_edge_up; l++)
                            {
                                
                                if(abs(k-i) <= inflation_num && abs(l-j) <= inflation_num)
                                {
                                    if(raw_costs_[k][l] < raw_costs_[i][j])
                                    {
                                        raw_costs_[k][l] = MAPCOSTS::INFLATED_OBSTACLE;
                                    }
                                }
                            }
                        }
                    }

                }
            }
        }

        bool CostMap::setupPlanningCostMap(double planning_res)
        {
            if(raw_size_x_ == 0)
                return false;
            resolution_ = planning_res;
            int scale = round(planning_res / raw_resolution_);

            size_x_ = raw_size_x_ / scale;
            size_y_ = raw_size_y_ / scale;

            costs_.resize(size_x_);
            for(int i = 0; i < size_x_; i++)
            {
                costs_[i].resize(size_y_);
            }

            int x_count = 0, y_count = 0;
            for(int x_raw = map_offset_x_; x_raw <= raw_size_x_ - scale; x_raw+=scale)
            {
                y_count = 0;
                for(int y_raw = map_offset_y_; y_raw <= raw_size_y_ - scale; y_raw+=scale)
                {
                    float obs_count = 0;
                    bool is_free = true;
                    for(int x = x_raw; x < x_raw + scale; x++)
                    {
                        for(int y = y_raw; y < y_raw + scale; y++)
                        {
                            if(x < 0 ||y < 0){
                                is_free = false;
                                break;
                            }
                            if(raw_costs_[x][y] != MAPCOSTS::FREE)
                            {
                                is_free = false;
                                if(raw_costs_[x][y] == MAPCOSTS::OBSTACLE)
                                    obs_count++;
                                break;
                            }
                        }
                        if(!is_free)
                            break;
                    }
                    // costs_[x_count][y_count] = obs_count / (scale*scale) > 0.3 ? MAPCOSTS::OBSTACLE : MAPCOSTS::FREE;
                    if(x_count < size_x_ && y_count < size_y_){
                        if(is_free){
                            costs_[x_count][y_count] = MAPCOSTS::FREE;
                        }else{
                            if(obs_count > 0)
                                costs_[x_count][y_count] = MAPCOSTS::OBSTACLE;
                            else
                                costs_[x_count][y_count] = MAPCOSTS::INFLATED_OBSTACLE;
                            
                        }
                    }
                    
                    y_count++;
                }
                x_count++;
            }

            is_ready_ = true;
            HIGHLIGHT("Created planning costmap with size "<<size_x_<<"*"<<size_y_<<" at resolution "<<resolution_)

            return is_ready_;
        }

    }
}